﻿using System;
using System.Diagnostics.Contracts;

namespace MathService.DSP
{
    [Copyright("Пример расчета цифрового фильтра нижних частот Баттерворта", url = "http://www.dsplib.ru/content/filters/butterex/butterex.html")]
    public class ButterworthLowpassFilter : CBIH
    {
        public static Polynom GetPolynom(int n)
        {
            Contract.Requires(n > 0);
            Contract.Ensures(Contract.Result<Polynom>() != null);
            Contract.Ensures(Contract.Result<Polynom>().Power == n);
            var N = n / 2;
            var r = n % 2;
            Func<int, Polynom> p = k => new Polynom(1, -2 * Math.Cos((2 * k + (double)n - 1) / 2 / n * Math.PI), 1);
            var result = new Polynom(1);
            for(var i = 1; i <= N; i++)
                result *= p(i);
            return r == 1 ? result * new Polynom(1, 1) : result;
        }

        public static Polynom[] GetPolynoms(int n)
        {
            Contract.Requires(n > 0);
            Contract.Ensures(Contract.Result<Polynom[]>() != null);
            Contract.Ensures(Contract.Result<Polynom[]>().Length == n / 2 + n % 2);
            var N = n / 2;
            var r = n % 2;
            Func<int, Polynom> p = k => new Polynom(1, -2 * Math.Cos((2 * k + (double)n - 1) / 2 / n * Math.PI), 1);
            var result = new Polynom[N + r];

            for(int k = 1, K = (n - r) / 2; k <= K; k++)
                result[k + r - 1] = p(k);
            if(r == 1) result[0] = Polynom.Koefficients(1, 1);

            return result;
        }

        public double Wp { get; private set; }
        public double Ws { get; private set; }
        public double Rp { get; private set; }
        public double Rs { get; private set; }
        public double dt { get; private set; }

        private static PQ Calculate(double wp, double ws, double Rp, double Rs, double dt)
        {
            Func<double, double> W = w => 2 / dt * Math.Tan(w / 2);
            var Wp = W(wp);
            var Ws = W(ws);

            Func<double, double> eps = R => Math.Sqrt(Math.Pow(10, R / 10) - 1);

            var eps_p = eps(Rp);
            var eps_s = eps(Rs);
            var Nd = Math.Log(eps_s / eps_p) / Math.Log(Ws / Wp);
            var N = //((int) (Nd))/2;
                (int)Math.Round(Nd); if(Nd > N) N++;

            const int r = 1;
            var L = (N - r) / 2;
            var alpha = Math.Pow(eps_p, -1 / (double)N);
            Func<int, double> thetta = n => (2 * n - 1) / (2 * (double)N) * Math.PI;
            var h = new Polynom(alpha, 1);
            var Q = new Polynom(1);
            for(var i = 0; i < r; i++) Q *= h;
            var alpha2 = alpha * alpha;
            Func<int, Polynom> p = n => new Polynom(alpha2, 2 * alpha * Math.Sin(thetta(n)), 1);
            for(var n = 1; n <= L; n++) Q *= p(n);
            //var P = new Polynom(1 / eps_p);
            Q = Q.ScalePolynom(1 / Wp) * eps_p;


            var pz1 = new Polynom(2 / dt, -2 / dt);
            var qz1 = new Polynom(1, 1);
            var pz = new Polynom(1);
            var qz = new Polynom(1);
            var ppz = new Polynom[N + 1];
            var qqz = new Polynom[N + 1];

            for(var i = 0; i <= N; i++)
            {
                ppz[i] = pz;
                pz *= pz1;
                qqz[N - i] = qz;
                qz *= qz1;
            }
            var Pz = qqz[0];
            var Qz = new Polynom(0);
            for(var i = 0; i <= N; i++)
                Qz += Q[i] * ppz[i] * qqz[i];
            var a0 = Qz[0];
            Qz /= a0;
            Pz /= a0;
            return new PQ(Pz, Qz);

        }

        private static PQ GetAnalogTransferFunction(double Wp, double Ws, double Rp, double Rs)
        {
            //Func<double, double> W = w => 2 / dt * Math.Tan(w / 2);
            //var Wp = W(wp);
            //var Ws = W(ws);

            Func<double, double> eps = R => Math.Sqrt(Math.Pow(10, R / 10) - 1);

            var eps_p = eps(Rp);
            var eps_s = eps(Rs);
            var Nd = Math.Log(eps_s / eps_p) / Math.Log(Ws / Wp);
            var N = //((int) (Nd))/2;
                (int)Math.Round(Nd); if(Nd > N) N++;

            const int r = 1;
            var L = (N - r) / 2;
            var alpha = Math.Pow(eps_p, -1 / (double)N);
            Func<int, double> thetta = n => (2 * n - 1) / (2 * (double)N) * Math.PI;
            var h = new Polynom(alpha, 1);
            var Q = new Polynom(1);
            for(var i = 0; i < r; i++) Q *= h;
            var alpha2 = alpha * alpha;
            Func<int, Polynom> p = n => new Polynom(alpha2, 2 * alpha * Math.Sin(thetta(n)), 1);
            for(var n = 1; n <= L; n++) Q *= p(n);
            //var P = new Polynom(1 / eps_p);
            Q = Q.ScalePolynom(1 / Wp) * eps_p;
            return new PQ(1, Q);
        }

        public ButterworthLowpassFilter(double wp, double ws, double Rp, double Rs, double dt)
            : this(Calculate(wp, ws, Rp, Rs, dt))
        {
            Wp = wp;
            Ws = ws;
            this.Rp = Rp;
            this.Rs = Rs;
            this.dt = dt;
        }

        private ButterworthLowpassFilter(PQ H) : base(H.Q.Coefficients, H.P.Coefficients) { }

    }
}